#include "ANO_DT.h"
#include "usart.h"
#include "ak8975.h"
#include "HC_SR04.h"
#include "RC.h"
#include "opticalflow.h"
#include "InertialNavigation.h"
#include "Disturbance_observer.h"
#include "Altitude.h"
#include "Position.h"
#include "Attitude.h"
#include "ADRC.h"
#include "Fusion.h"
#include "Mixer.h"
#include "Adp_ADRC.h"
#include "SDK.h"
#include "GPS.h"

//数据拆分宏定义，在发送大于1字节的数据类型时，比如int16、float等，需要把数据拆分成单独字节进行发送
#define BYTE0(dwTemp)       ( *( (char *)(&dwTemp)		) )
#define BYTE1(dwTemp)       ( *( (char *)(&dwTemp) + 1) )
#define BYTE2(dwTemp)       ( *( (char *)(&dwTemp) + 2) )
#define BYTE3(dwTemp)       ( *( (char *)(&dwTemp) + 3) )

extern FLOAT_XYZ Acc_filt,Gyr_rad;	         //加速度
extern FLOAT_ANGLE Att_Angle;
extern float TD_rol,TD_pit;
extern RC_Dat RC_Data;
extern Altimeter Ultra_US100;    
extern alt_control Alt_control;
extern float Ultra_Hnow,Ultra_V;
extern Vector2f_t up302_w,up302_w_filted;
extern Vector2f_t up302_Gyr_Filted,up302_v,up302_v_filted;
extern Vector2f_t up302_position,up302_position_filted;
extern Vector2f_t ACC_XY_Navigation,Acc_XYFrame;
extern NDO High_NDO;
extern uint16_t PPM_Databuf[10];//ppm数据的十个通道储存位置
extern float LockBuf_,ModeBuf_;
extern pos_control Sys_Pos_Control; 
extern att_control Sys_Att_Control;
extern float w1,w2,w3,w4;
extern float W;
extern ADRC_TD_Def Slide_TargetROL2d,Slide_TargetPIT2d;
extern char UWB_Data[];
extern int16_t X_Pos,Y_Pos;
extern float X_FPos,Y_FPos;
extern float Ultra_height;

extern PID_TYPE PID_POS_X,PID_POS_Y;

uint8_t V7data[50];	  //发送数据缓存

/* @brief  发送加速度计、陀螺仪传感器数据(Acc_filt,Gyr_rad)
 * @param  
**/
void ANO_DT_Send_IMUSensor(float a_x,float a_y,float a_z,float g_x,float g_y,float g_z)
{
	uint8_t _cnt=0;    
	int16 _temp;
	
	V7data[_cnt++]=HEAD;
	V7data[_cnt++]=D_ADDR;    
	V7data[_cnt++]=Function_IMUSensor; //
    
	V7data[_cnt++]=13;     //length
   
	_temp = (int16)(100*a_x);
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
       
	_temp = (int16)(100*a_y);
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);

	_temp = (int16)(100*a_z);	
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
	
	_temp = (int16)(100*g_x);	
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);

	_temp = (int16)(100*g_y);		
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);

	_temp = (int16)(100*g_z);	
    V7data[_cnt++]=BYTE0(_temp);  
	V7data[_cnt++]=BYTE1(_temp);
                                                                                                
    V7data[_cnt++]=1;     //震动状态

	uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
    Usart_Send(V7data, _cnt);
}

/* @brief  发送欧拉角(Att_Angle)
 * @param 
**/
void ANO_DT_Send_Attangle(FLOAT_ANGLE *angle_in)
{
	uint8_t _cnt=0;
	int16 _temp;

	V7data[_cnt++]=HEAD;    
	V7data[_cnt++]=D_ADDR;
	V7data[_cnt++]=Function_Attangle;  
    V7data[_cnt++]=7;  
	
	_temp = (int16)(angle_in->rol*100); 
	V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
	_temp = (int16)(angle_in->pit*100);
	V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
	_temp = (int16)(angle_in->yaw*100);
	V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
		
	V7data[_cnt++] = 1;
	
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
    Usart_Send(V7data, _cnt);

}


/* @brief  自定义发送加速度
 * @param 
**/
void V7User_acc(float ACC_RAW,float Accfilt)
{
	u8 _cnt=0;
	s32 _temp;

	V7data[_cnt++]=HEAD;
	V7data[_cnt++]=D_ADDR;
    V7data[_cnt++]=User_1;

	V7data[_cnt++]=0;

    _temp = (s16)(100*ACC_RAW);
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
   
    _temp = (s16)(100*Accfilt);
    V7data[_cnt++]=BYTE0(_temp);
	V7data[_cnt++]=BYTE1(_temp);
    
	V7data[3] = _cnt-4;
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
    Usart_Send(V7data, _cnt);

}

extern float VTest;
extern FLOAT_ANGLE MeaANGLE_d,S_fun; 
extern float CX,CY,CZ;
extern FLOAT_ANGLE TarANGLE_d,TarANGLE_2d;    //目标角度的一阶微分与二阶微分
extern FLOAT_ANGLE ANGLE_Error,ANGLE_Error_d; 
extern uint16_t PID_PWM1,PID_PWM2,PID_PWM3,PID_PWM4;
extern float PWM_RAW[4],rotor_output[4];
extern Vector3f_t C_RAW,U;


extern float outputs[4];
extern float min_out,max_out;
extern Altimeter Laser_TOF2m;
extern uint16_t THR_Out;
extern uint16_t M_PWM1,M_PWM2,M_PWM3,M_PWM4; //四电机PWM
extern float rol_pitch_out[4];
extern float ROL_OUT,PIT_OUT,YAW_OUT; 
extern float thrust;//推力
extern SCALE_TYPE Rotor_Scale[4];
extern float target_alt;
extern float LC302wx,LC302wy;
extern uint8_t tx1;
extern int16_t X_PosTar,Y_PosTar,Z_PosTar;
extern Auto_Con Sys_Auto_Control;
extern u16 size;
extern float X_PosTar_Body,Y_PosTar_Body;
extern float X_posb,Y_posb;
extern float err_x;


void V7User_LightFlow(void) 
{
	u8 _cnt=0;
	s16 temp16;
	s32 temp32;

	V7data[_cnt++]=HEAD;
	V7data[_cnt++]=D_ADDR;
    V7data[_cnt++]=User_5;

    V7data[_cnt++]=0;
//	temp16 = (s16)(up302_w.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(up302_w.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(up302_w_filted.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(up302_w_filted.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(LC302wx*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(LC302wy*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(up302_Gyr_Filted.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(up302_Gyr_Filted.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);


//	temp16 = (s16)(LockBuf_*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(ModeBuf_*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);	

//	temp16 = (s16)(X_Pos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(X_FPos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(X_PosTar*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Y_Pos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Y_FPos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(tx1*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(up302_v.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(up302_v.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(Acc_XYFrame.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Acc_XYFrame.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(up302_v_filted.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(up302_v_filted.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(Sys_Pos_Control.posOuterTarget.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Sys_Pos_Control.posOuterTarget.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
		
//	temp16 = (s16)(Sys_Pos_Control.posOuterMeasure.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Sys_Pos_Control.posOuterMeasure.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Sys_Pos_Control.posInnerTarget.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Sys_Pos_Control.posInnerTarget.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);	
//	
//	temp16 = (s16)(Sys_Pos_Control.posInnerMeasure.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(X_Pos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Y_Pos*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(X_Pos);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Y_Pos);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(X_posb);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Y_posb);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(X_FPos);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Y_FPos);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp32 = (s32)(X_FPos);        
//    V7data[_cnt++]=BYTE0(temp32);
//	V7data[_cnt++]=BYTE1(temp32);
//	V7data[_cnt++]=BYTE2(temp32);
//	V7data[_cnt++]=BYTE3(temp32);
//	
//	temp32 = (s32)(Y_FPos);        
//    V7data[_cnt++]=BYTE0(temp32);
//	V7data[_cnt++]=BYTE1(temp32);
//	V7data[_cnt++]=BYTE2(temp32);
//	V7data[_cnt++]=BYTE3(temp32);
	
//	temp16 = (s16)(X_PosTar*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Y_PosTar*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(X_PosTar_Body);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Y_PosTar_Body);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	

	temp16 = (s16)(PID_POS_X.Error);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(err_x);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	
	
	temp16 = (s16)(Sys_Pos_Control.posInnerTarget.x*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	
	
//	
//	temp16 = (s16)(Sys_Pos_Control.posInnerMeasure.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(up302_position.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(up302_position.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(up302_position_filted.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(up302_position_filted.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
                               	
//	temp16 = (s16)(Sys_Att_Control.attOuterTarget.rol*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(Sys_Att_Control.attOuterTarget.pit*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);

	
//	temp16 = (s16)(Target_V.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Target_V.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(Pos_CtlMoveFlag*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Sys_Pos_Control.posOuterTarget.x*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Sys_Pos_Control.posOuterTarget.y*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
		
//	temp16 = (s16)(Att_Angle.rol*100);        
//  V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(Att_Angle.pit*100);        
//	V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);	
		
//	temp16 = (s16)(Att_Angle.yaw*100);        
//	V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(TD_rol*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(TD_pit*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	
//	temp16 = (s16)(Sys_Auto_Control.Enable*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Ultra_height*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Ultra_US100.Distance_now*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Ultra_US100.Vertical_Dimension_now*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(Ultra_Hnow*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(Ultra_V*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Alt_control.Target_alt*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(Alt_control.Target_velocity*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);



	
	temp16 = (s16)(THR_Out);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	

	V7data[3] = _cnt-4;
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
	size = _cnt;
	
    //Usart_Send(V7data, _cnt);

}

extern ADP_ADRC_Struct ADRC_ROL_RATE;
extern PID_TYPE PID_ROL_Angle;
extern float e1_fal,e2_fal;
extern Div_S ESO_Z3;


void V7User_F6(void) 
{
	uint8_t _cnt=0;
	s16 temp16;


	V7data[_cnt++]=HEAD;
	V7data[_cnt++]=D_ADDR;
    V7data[_cnt++]=User_6;

    V7data[_cnt++]=0;
	
//	temp16 = (s16)(PID_ROL_Angle.OutPut*100);        
//  V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
		
//	temp16 = (s16)(RC_Data.RC_ROLL*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(ADRC_ROL_RATE.td_v1*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(ADRC_ROL_RATE.td_v2*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

	temp16 = (s16)(Gyr_rad.X*RadtoDeg*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(ADRC_ROL_RATE.td_v1*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(ADRC_ROL_RATE.e1*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(e1_fal*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(ADRC_ROL_RATE.e2*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(e2_fal*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
    //	temp16 = (s16)(ADRC_ROL_RATE.td_v2*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
//	
//	temp16 = (s16)(ADRC_ROL_RATE.e2*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(ADRC_ROL_RATE.ESO.z1*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
//	temp16 = (s16)(ADRC_ROL_RATE.ESO.e*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);

//	temp16 = (s16)(ESO_Z3.div*100);        
//    V7data[_cnt++]=BYTE0(temp16);
//	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(ESO_Z3.div_filt*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
		
	temp16 = (s16)(ADRC_ROL_RATE.ESO.z2*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(ADRC_ROL_RATE.ESO.z3*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	temp16 = (s16)(ADRC_ROL_RATE.u0*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16); 
	
	temp16 = (s16)(ADRC_ROL_RATE.u*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16); 
	
	
	V7data[3] = _cnt-4;
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
    Usart_Send(V7data, _cnt);
	
}

extern gps_data GPS;
void V7User_Ligh(void) 
{
	u8 _cnt=0;
	s16 temp16;
	s32 temp32;

	V7data[_cnt++]=HEAD;
	V7data[_cnt++]=D_ADDR;
    V7data[_cnt++]=User_1;

    V7data[_cnt++]=0;

	temp16 = (s16)(Ultra_Hnow*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);

	
	temp16 = (s16)(GPS.numSV*100);        
    V7data[_cnt++]=BYTE0(temp16);
	V7data[_cnt++]=BYTE1(temp16);
	
	V7data[3] = _cnt-4;
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
    for(uint8_t i=0;i<(V7data[3]+4);i++)
    {
        sumcheck += V7data[i]; //从帧头开始，对每一字节进行求和，直到DATA区结束
        addcheck += sumcheck; //每一字节的求和操作，进行一次sumcheck的累加
    }

    V7data[_cnt++]=sumcheck;
    V7data[_cnt++]=addcheck;
	size = _cnt;
	
    //Usart_Send(V7data, _cnt);

}

extern FLOAT_XYZ Acc_Butter,Gyr_Butter;
extern Vector3f_t Acc_BodyNavigation,ACC_w;

void ANO_DT_Data_Exchange(void)
{
//	ANO_DT_Send_IMUSensor(Acc_filt.X,Acc_filt.Y,Acc_filt.Z,Gyr_rad.X,Gyr_rad.Y,Gyr_rad.Z);
//    ANO_DT_Send_Attangle(&Att_Angle);
	
	
//	V7User_acc(Acc_filt.Z,ACC_w.z);
//	V7User_LightFlow();
//	V7User_F6();
	
	V7User_Ligh();
}

